// Copyright 2006-2012 Brendan Burns
//
// Permission is hereby granted, free of charge, to any person obtaining
// a copy of this software and associated documentation files (the
// "Software"), to deal in the Software without restriction, including
// without limitation the rights to use, copy, modify, merge, publish,
// distribute, sublicense, and/or sell copies of the Software, and to
// permit persons to whom the Software is furnished to do so, subject to
// the following conditions:
//
// The above copyright notice and this permission notice shall be
// included in all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
// EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
// MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
// NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE
// LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
// OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION
// WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
#include <errno.h>
#include <termios.h>
#include <unistd.h>
#include <stdio.h>
#include <sys/time.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <stdlib.h>
#include "nmc_low_level.h"
#include "nmc_status.h"
#include "serial_util.h"

#define DEBUG 1

int nmc_read_response(nmc_port *port, int length, char *msg);

/**
 * Calculate a checksum and then write a message
 * assumes that msg contains space for the checksum.
 * \param fd File descriptor to write to.
 * \param msg The message array (at least size+1 bytes long)
 * \param size The size of the message in bytes
 **/
void nmc_write_serial(nmc_port *port, char *msg, int size) {
  int i;
  int wrote;
  
  msg[size] = 0;
  for (i=1;i<size;i++) {
    msg[size] += msg[i];
  }
  write_serial(port->fd, msg, size+1);
}

/**
 * Print the flags in a status byte
 * \param byte The status byte.
 **/
void print_status(char byte) {
  if (byte & MOVE_DONE) 
    printf("MOVE DONE |");
  if (byte & CHKSUM_ERROR)
    printf ("CHECKSUM ERROR |");
  if (byte & OVERCURRENT)
    printf("OVERCURRENT |");
  if (byte & POWER_ON)
    printf("POWER ON |");
  if (byte & POSITION_ERROR)
    printf("POSITION ERROR |");
  if (byte & LIMIT_1)
    printf("LIMIT 1 |");
  if (byte & LIMIT_2)
    printf("LIMIT 2 |");
  if (byte & HOME_IN_PROGRESS)
    printf("HOME IN PROGRESS |");
  if (byte)
    printf("\n");
}

/**
 * Read the status message response.
 * \param fd The file to read
 * \param ad The address of the module
 **/
int nmc_read_status(nmc_port *port, int ad) {
  int numbytes = 2;       //start with stat & cksum
  int addr = ad - 1;
  char msg[32];
  int bytecount;
  
  if ( (port->modules[addr].status) & SEND_POS )	numbytes +=4;
  if ( (port->modules[addr].status) & SEND_AD ) 	numbytes +=1;
  if ( (port->modules[addr].status) & SEND_VEL ) numbytes +=2;
  if ( (port->modules[addr].status) & SEND_AUX ) numbytes +=1;
  if ( (port->modules[addr].status) & SEND_HOME ) numbytes +=4;
  if ( (port->modules[addr].status) & SEND_ID ) 	numbytes +=2;
  if ( (port->modules[addr].status) & SEND_PERROR ) numbytes +=2;
  if ( (port->modules[addr].status) & SEND_NPOINTS ) numbytes +=1;

  nmc_read_response(port, numbytes, msg);
  port->modules[addr].status_byte = msg[0];
  if(msg[0] & CHKSUM_ERROR) {
    fprintf(stderr, "Read status checksum error!\n");
    return -1;
  }
  
  //Finally, fill in status data
  bytecount = 1;
  if ( (port->modules[addr].status) & SEND_POS )
    {
      port->modules[addr].mod.servo.position = *( (long *)(msg + bytecount) );
      bytecount +=4;
    }
  if ( (port->modules[addr].status) & SEND_AD )
    {
      port->modules[addr].mod.servo.ad = msg[bytecount];
      bytecount +=1;
    }
  if ( (port->modules[addr].status) & SEND_VEL )
    {
      port->modules[addr].mod.servo.velocity = *( (short int *)(msg + bytecount) );
      bytecount +=2;
    }
  if ( (port->modules[addr].status) & SEND_AUX )
    {
      //p->aux = inbuf[bytecount];
      bytecount +=1;
    }
  if ( (port->modules[addr].status) & SEND_HOME )
    {
      //p->home = *( (unsigned long *)(inbuf + bytecount) );
      bytecount +=4;
    }
  if ( (port->modules[addr].status) & SEND_ID )
    {
      //port->modules[addr].modtype = inbuf[bytecount];
      //port->modules[addr].modver = inbuf[bytecount+1];
      bytecount +=2;
    }
  if ( (port->modules[addr].status) & SEND_PERROR )
    {
      port->modules[addr].mod.servo.error = *( (short int *)(msg + bytecount) );
      bytecount +=2;
    }
  if ( (port->modules[addr].status) & SEND_NPOINTS )
    {
      port->modules[addr].mod.servo.npoints = msg[bytecount];
    }
  return 0;
}  

/**
 * Read the status of a module (Pg. 26)
 * \param fd The file to read/write
 * \param addr The addres of the module
 * \param flags The status items to get.
 **/
int nmc_send_read_status(nmc_port *port, int addr, char flags) {
  char msg[8];
  int res;
  
  // Store old status
  int old_status = port->modules[addr-1].status;
  // Build message
  msg[0] = 0xAA;
  msg[1] = addr;
  msg[2] = 0x13;
  msg[3] = flags;
  nmc_write_serial(port, msg, 4);
  // Parse message
  port->modules[addr-1].status = flags;
  res = nmc_read_status(port, addr);
  // Restore old status
  port->modules[addr-1].status = old_status;
  
  return res;
}

/**
 * Reset the home position (Pg. 23)
 * \param fd The file to read/write
 * \param addr The address of the module
 **/
int nmc_reset(nmc_port *port, int addr) {
  char msg[4];
  msg[0] = 0xAA;
  msg[1] = addr;
  msg[2] = 0x00;
  
  nmc_write_serial(port, msg, 3);
  return nmc_read_status(port, addr);
}

/**
 * Adjust the motor parameters (Pg. 33) 
 * Does not support fourth mode.
 * \param fd The file to read/write
 * \param addr The address of the module
 * \param mode The motor mode
 **/
int nmc_motor_stop(nmc_port *port, int addr, char mode) {
  char msg[8];
  msg[0] = 0xAA;
  msg[1] = addr;
  msg[2] = 0x17;
  msg[3] = (mode &= (~STOP_HERE));

  nmc_write_serial(port, msg, 4);
  return nmc_read_status(port, addr);
}

/**
 * Set the group address of a module (Pg. 24)
 * \param fd The file to read/write
 * \param addr The address of the module
 * \param id The new group id
 * \param leader Is this module the leader of the group?
 **/
int nmc_set_group_internal(nmc_port *port, int addr, int id, int leader) {
  char msg[8];
  
  msg[0] = 0xAA;
  msg[1] = addr;
  msg[2] = 0x21;
  msg[3] = addr;
  msg[4] = (leader? id &= 0x7F: id);

  port->modules[addr-1].group = id;

  nmc_write_serial(port, msg, 5);
  return nmc_read_status(port, addr);
}

/**
 * Define the status data sent back (Pg. 25)
 * \param fd The file to read/write
 * \param addr The address of the module
 * \param flags The status flags
 **/
int nmc_define_status(nmc_port *port, int addr, int flags) {
  char msg[8];
    
  msg[0] = 0xAA;
  msg[1] = addr;
  msg[2] = 0x12;
  msg[3] = flags;
  
  nmc_write_serial(port, msg, 4);
  port->modules[addr-1].status = flags;
  nmc_read_status(port, addr);
    
  return 0;
}


int nmc_init_module(nmc_port *port, int addr) {
  int numread;
  char msg[8];
  msg[0] = 0xAA;
  msg[1] = addr;
  msg[2] = 0x13;
  msg[3] = 0x20;
 
  nmc_write_serial(port, msg, 4);
  
  nmc_read_response(port, 4, msg);
#ifdef DEBUG
  print_status(msg[0]);
#endif
  port->modules[addr-1].status_byte = msg[0];
  port->modules[addr-1].type = msg[1];
  port->modules[addr-1].version = msg[2];
  port->modules[addr-1].group = 0xFF;
  port->modules[addr-1].status = 0x00;
  port->modules[addr-1].mod.servo.ioctrl = 0;
}

int nmc_read_response(nmc_port *port, int length, char *msg) {
  int numread = read_serial(port->fd, msg, length);  //get back status, cksum
  char chksum = 0;
  int i;

  if (numread!=length) {
    printf("Failed to read response!!\n");
    if (numread == 2)
      print_status(msg[0]);
    return -1;
  }
  for(i=0;i<length-1;i++) {
    chksum += msg[i];
  }
  if (chksum != msg[length-1]) {
    printf("Status checksum error %d %d - please reset the Network\n",
	   chksum, msg[length-1]);
    return -2;
  }
  return 0;
}

int current_baud;
/**
 * Change the baud rate of communication (Pg. 37)
 * \param fd The file to read/write
 * \param baud The new speed
 **/
int nmc_change_baud(nmc_port *port, int baud) {
  char msg[16];
  int baudrate;
  struct termios newtio;

  if (baud == current_baud) {
    return 0;
  }

  msg[0] = 0xAA;
  msg[1] = 0xFF;
  msg[2] = 0x1A;

  switch(baud) {
  case 9600:
    baudrate = B9600;
    msg[3] = 127;
  case 19200:
    baudrate = B19200;
    msg[3] = 64;
    break;
  case 57600:
    baudrate = B57600;
    msg[3] = 21;
    break;
  case 115200:	
    baudrate = B115200;
    msg[3] = 10;
    break;
  case 230400:
    baudrate = B230400;
    msg[3] = 5;
    break;
  default:
    fprintf(stderr, "Error, invalid baud rate: %d.  Using default.\n", baud);
    return -1;
  }

  nmc_write_serial(port, msg, 4);
  tcdrain(port->fd);
  usleep(100*1000);

  tcgetattr(port->fd, &newtio);
  
  cfsetispeed(&newtio, baudrate);
  cfsetospeed(&newtio, baudrate);
  
  tcsetattr(port->fd, TCSANOW, &newtio);
  tcflush(port->fd, TCIOFLUSH);
}

/**
 * Connect to the nmc network.
 * \param port The file to open
 * \param baud The desired baudrate
 * \param nummod Output parameter, number of modules
 * \returns The file descriptor
 **/
nmc_port *nmc_connect(const char *file, int baud, int *nummod) {
  int ret;
  int addr;
  char msg[20];
  int i;
  struct termios newtio;
  nmc_port *port = (nmc_port *)malloc(sizeof(nmc_port));
  
#ifdef DEBUG
  printf("Opening %s\n",file);
#endif
  //port->fd = open(file, O_RDWR | O_NOCTTY);
  port->fd = open(file, O_RDWR);
  if (port->fd < 0)
    {
      printf("%s failed to open\n", file);
      perror("NMC_connect"); 
      return 0;
    }
 
  tcgetattr(port->fd,&newtio); // save current port settings
  newtio.c_cflag = CS8 | CLOCAL | CREAD;
  newtio.c_cflag |= B19200;
  newtio.c_iflag = IGNPAR;
  newtio.c_oflag = 0;
  // set input mode (non-canonical, no echo,...)
  newtio.c_lflag = 0;
  
  // Wait block for 100ms, then timeout
  newtio.c_cc[VTIME]    = 1;   /* inter-character timer unused */
  newtio.c_cc[VMIN]     = 0;   /* blocking read until 1 chars received */
  
  tcflush(port->fd, TCIOFLUSH);
  ret = tcsetattr(port->fd,TCSANOW,&newtio);
  if (ret < 0)
    {
      printf("Failed to set COMM configuration (%d)\n", errno);
    }
  
  // Clear out buffers
  for (i=0;i<20;i++) {
    msg[i] = 0;
  }
  write_serial(port->fd, msg, 20);
  // Wait for settling
  usleep(10*1000);
  
  // Set enumerate modules
  addr = 1;
  const int MAX_STATUS = 10;
  char status[MAX_STATUS];
  while (1) {
    int numread;
    //First set the address to a unique value:
    msg[0] = 0xAA;		//Header
    msg[1] = 0;  		//Send to default address of 0
    msg[2] = 0x21;
    msg[3] = addr;  	//Set new address sequentially
    msg[4] = 0xFF;		//Set group address to 0xFF
    
    nmc_write_serial(port, msg, 5);

    numread = read_serial(port->fd, msg, 2);  //get back status, cksum
    if (numread!=2) {
      break;	//if no response, punt out of loop
    }
    if (msg[0] != msg[1]) {
      printf("Status checksum error - please reset the Network\n");
      return(0);
    }
    
#ifdef DEBUG
    printf("Module: %d\n", addr);
    print_status(msg[0]);
#endif
    if (addr < MAX_STATUS) {
      status[addr] = msg[0];
    }
    addr++;
  }
  *nummod = addr-1;
  port->modules = (module_t *)malloc((addr-1)*sizeof(module_t));
  for (int i = 0; i < *nummod && i < MAX_STATUS; ++i) {
    port->modules[i].status_byte = status[i];
  }
  current_baud = 19200;
  
  nmc_change_baud(port, baud);
  
  return port;
}

/**
 * Start a motion previously loaded via Load Trajectory (Pg. 30)
 * \param fd The file to read/write
 * \param group The group of modules to start
 * \param leader The leader of the group.
 **/
int nmc_start_motion(nmc_port *port, char group, char leader) {
  char msg[4];
  msg[0] = 0xAA;
  msg[1] = group;
  msg[2] = 0x05;
  
  nmc_write_serial(port, msg, 3);
  return nmc_read_status(port, leader);
}

/**
 * Save the current position as home (Pg. 39)
 * \param fd The file to read/write
 * \param group The group to send the command to
 * \param leader The address of the group leader
 **/
int nmc_save_home(nmc_port *port, char group, char leader) {
  char msg[4];
  msg[0] = 0xAA;
  msg[1] = group;
  msg[2] = 0x0C;
  
  nmc_write_serial(port, msg, 3);
  return nmc_read_status(port, leader);
}

/**
 * Start a stored path (Pg. 40)
 * \param fd The file to read/write
 * \param group The group holding the path
 * \param leader The leader of the group
 **/
int nmc_start_path_internal(nmc_port *port, char group, char leader) {
  char msg[4];
  msg[0] = 0xAA;
  msg[1] = group;
  msg[2] = 0x0D;
    
  nmc_write_serial(port, msg, 3);
  return nmc_read_status(port, leader);
}

/**
 * Set the gains on the PID controller (Pg. 31)
 * \param fd The file to read/write
 * \param kp Propotional gain
 * \param ki Integral gain
 * \param kd Derivative gain
 * \param integration_limit Maximum value for integral term
 * \param output_limit Maximum pwm output
 * \param current_limit Maximum current output 
 * \param error_limit Maximum error value
 * \param rate_divisor See section 4.3 of manual
 * \param deadband_compensation See section 4.3 of manual
 **/
int nmc_set_gain(nmc_port *port, uint addr, short int kp, short int ki, short int kd, 
		 short int integration_limit, char output_limit, 
		 char current_limit, short int error_limit, char rate_divisor, 
		 char deadband_compensation, char step_rate_multiplier)
{
  char msg[32];
  char *cmdstr = msg+3;
    
  msg[0] = 0xAA;
  msg[1] = addr;
  msg[2] = 0xF6;
  
  *( (short int *)(cmdstr) ) = kp;
  *( (short int *)(cmdstr+2) ) = kd;
  *( (short int *)(cmdstr+4) ) = ki;
  *( (short int *)(cmdstr+6) ) = integration_limit;
  *( (char *)(cmdstr+8) ) = output_limit;
  *( (char *)(cmdstr+9) ) = current_limit;
  *( (short int *)(cmdstr+10) ) = error_limit;
  *( (char *)(cmdstr+12) ) = rate_divisor;
  *( (char *)(cmdstr+13) ) = deadband_compensation;
  *(cmdstr+14) = step_rate_multiplier;
  
  nmc_write_serial(port, msg, 18);
  return nmc_read_status(port, addr);
}

/**
 * Send a motion to the controller. (Pg. 27)
 * \param fd The file to read/write
 * \param addr The address of the module
 * \param mode The mode of the command
 * \param pos The commanded position (if mode is set position)
 * \param vel The commanded velocity (if mode is set velocity)
 * \param acc The commanded acceleration (if mode is set acc.)
 * \param pwm The commanded pwm (if mode is raw)
 **/
int nmc_servo_load_traj(nmc_port *port, char addr, char mode, long pos, 
			long vel, long acc, char pwm, bool wait)
{
  char cmdstr[24];
  int count;
  cmdstr[0] = 0xAA;
  cmdstr[1] = addr;
  cmdstr[2] = LOAD_TRAJ;
  
  count = 3;
  cmdstr[count] = mode; count++;
  if (mode & LOAD_POS) { *( (long *)(cmdstr + count) ) = pos; count += 4; }
  if (mode & LOAD_VEL) { *( (long *)(cmdstr + count) ) = vel; count += 4; }
  if (mode & LOAD_ACC) { *( (long *)(cmdstr + count) ) = acc; count += 4; }
  if (mode & LOAD_PWM) { *( (char *)(cmdstr + count) ) = pwm; count += 1; }
  
  cmdstr[2] = (char)((((count-3)<<4) & 0xF0) | LOAD_TRAJ);
  nmc_write_serial(port, cmdstr, count);
  if (wait)
    return nmc_read_status(port, addr);
  else {
    return 0;
  }
}

/**
 * Add path points (Pg. 40) Maximum 7 points.
 * \param fd The file to read/write
 * \param addr The address of the module
 * \param count The number of points
 * \param path The set of points.
 **/
int nmc_add_pathpoints(nmc_port *port, int addr, int count, 
		       int *path, char freq)
{
  char msg[24];
  char *cmdstr = msg+3;
  int16_t diff;
  char rev;
  int i;
  module_t *mod = &(port->modules[addr-1]);
  
  msg[0] = 0xAA;
  msg[1] = addr;
  
  for (i=0; i<count; i++)
    {
      diff = path[i] - mod->mod.servo.last_point;
      if (diff<0)
	{
	  rev = 0x01;
	  diff = -diff;
	}
      else 
	rev = 0x00;

      //Scale the difference appropriately for path freq. used
      if (mod->mod.servo.ioctrl & FAST_PATH)  //scale for 60/120 Hz fast path
	{
	  if (freq == P_60HZ)
	    {
	      diff *= (256/32);
	      diff |= 0x02;     //60 Hz -> set bit 1 = 1
	    }
	  else if (freq == P_120HZ) 
	    diff *= (256/16);
	  else 
	    return -1;
	}
      else  //scale for 30/60 Hz slow path
	{
	  if (freq == P_30HZ)
	    {
	      diff *= (256/64);
	      diff |= 0x02;     //30 Hz -> set bit 1 = 1
	    }
	  else if (freq == P_60HZ) 
	    diff *= (256/32);
	  else return -1;
	}
      
      diff |= rev;  //bit 0 = reverse bit

      *( (int16_t *)(cmdstr + 2*i) ) = (int16_t)diff;

      mod->mod.servo.last_point = path[i];
    }
  msg[2] = (char)((((2*count)<<4) & 0xF0) | 0x0D);

  nmc_write_serial(port, msg, count*2+3);
  return nmc_read_status(port, addr);
}

/**
 * No Op, simply returns status. (Pg. 42)
 * \param fd The file to read/write
 * \param addr The address
 **/
int nmc_no_op_internal(nmc_port *port, char addr) {
  char msg[4];
  msg[0] = 0xAA;
  msg[1] = addr;
  msg[2] = 0x0E;
  nmc_write_serial(port, msg, 3);
  return nmc_read_status(port, addr);
}

/**
 * Hard reset (Pg. 43) Resets baud to 19200
 * \param fd The file to read/write
 * \param group The group
 **/
void nmc_hard_reset(nmc_port *port, char group) {
  char msg[21];
  int i;
  struct termios newtio;
  // Clear out the buffer
  for (i=0;i<20;i++) {
    msg[i] = 0;
  }
  write_serial(port->fd, msg, 20);
  
  // Create the reset message
  msg[0] = 0xAA;
  msg[1] = group;
  msg[2] = 0x0F;
  
  // Send it
  nmc_write_serial(port, msg, 3);
  
  usleep(100*1000);
  tcgetattr(port->fd, &newtio);
  cfsetispeed(&newtio, B19200);
  cfsetospeed(&newtio, B19200);
  
  tcsetattr(port->fd, TCSANOW, &newtio);
  tcflush(port->fd, TCIOFLUSH);
}

/*
int main(int argc, char **argv) {
  int ret;
  int fd;
  int i;
  char msg[20];
  int nummod;
  int type, version;
  long counter  = 0;
  struct timeval start, now;
  
  fd = nmc_connect(argv[1], 115200, &nummod);
  for (i=1;i<=nummod;i++) {
    nmc_init_module(fd, i);
    printf("%d: %d %d\n", i, modules[i-1].type, modules[i-1].version);
  }

  
  for (i=1;i<=nummod;i++) {
    nmc_define_status(fd, i, SEND_POS);
  }
  
  for (i=1;i<=nummod;i++) {
    nmc_motor_stop(fd, i, AMP_ENABLE | MOTOR_OFF);
    nmc_motor_stop(fd, i, AMP_ENABLE | STOP_ABRUPT);
    nmc_reset(fd, i);
  }
  
  nmc_set_group(fd, 1, 0x81, 1);
  nmc_set_group(fd, 2, 0x81, 0);
  nmc_set_group(fd, 3, 0x81, 0);
  nmc_set_group(fd, 4, 0x81, 0);
  nmc_set_group(fd, 5, 0x81, 0);


   gettimeofday(&start, NULL);
   while (1) {
   for (int i=1;i<=nummod;i++) {
   nmc_get_status(fd, i);
   }
   if ((++counter % 100) == 0) {
   gettimeofday(&now, NULL);
   double elapsed = now.tv_sec-start.tv_sec+
   ((float)(now.tv_usec-start.tv_usec))/1000000;
   printf("Time: %g  Average: %g  Hz: %g %ld\n", 
   elapsed, (elapsed/counter), 1/(elapsed/counter), counter);
   }
   }
 
 close(fd);
}
*/    

int nmc_zero_encoder(nmc_port *port, int addr) {
  char msg[4];
  msg[0] = 0xAA;
  msg[1] = addr;
  msg[2] = 0x00;
  nmc_write_serial(port, msg, 3);
  return nmc_read_status(port, addr);
}
